﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;

namespace Drifting.MotionPlanning.Systems
{
    public class Drift8D : PlanarEnvironment
    {
        double g;    //m/s/s
        double m;    //kg
        double I_z;  //kg m2
        double l_F;  //m
        double l_R;  //m
        double r;    //m
        double I_F;  //kg m2
        double I_R;  //kg m2
        double h;    //m
        double B;
        double C;
        double D;

        public Drift8D()
            : base("../../../MotionPlanning/Resources/CarEnvironment.kdl", "../../../MotionPlanning/Resources/Drift8D.kdl")
        {
            g = 9.80665;     //m/s/s
            m = 1450;        //kg
            I_z = 2740;      //kg m2
            l_F = 1.3805;       //m
            l_R = 1.3805;       //m
            r = 0.3;         //m
            I_F = 1.8;       //kg m2
            I_R = 1.8;       //kg m2
            h = 0.4;         //m
            B = 7;
            C = 1.6;
            D = 0.52;
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            double dl = ctrl[0]; // dl is the steering angle of the front wheel
            double T_F = ctrl[1];// T_F front wheel torque
            double T_R = ctrl[2];// T_R rear wheel torque

            double x = q[0];
            double y = q[1];
            double fi = q[2];
            double xd = q[3];
            double yd = q[4];
            double fid = q[5];
            double w_F = q[6];
            double w_R = q[7];

            double V = Math.Sqrt(xd * xd + yd * yd);
            double beta = Math.Atan2(yd, xd) - fi;
            double V_Fx = V * Math.Cos(beta - dl) + fid * l_F * Math.Sin(dl);
            double V_Fy = V * Math.Sin(beta - dl) + fid * l_F * Math.Cos(dl);
            double V_Rx = V * Math.Cos(beta);
            double V_Ry = V * Math.Sin(beta) - fid * l_R;

            double s_Fx = (V_Fx - w_F * r) / (w_F * r);
            double s_Fy = V_Fy / (w_F * r);
            double s_Rx = (V_Rx - w_R * r) / (w_R * r);
            double s_Ry = V_Ry / (w_R * r);
            double s_F = Math.Sqrt(s_Fx * s_Fx + s_Fy * s_Fy);
            double s_R = Math.Sqrt(s_Rx * s_Rx + s_Ry * s_Ry);

            double mu_F__s_F = D * Math.Sin(C * Math.Atan(B * s_F));
            double mu_R__s_R = D * Math.Sin(C * Math.Atan(B * s_R));
            double mu_Fx = -s_Fx / s_F * mu_F__s_F;
            double mu_Fy = -s_Fy / s_F * mu_F__s_F;
            double mu_Rx = -s_Rx / s_R * mu_R__s_R;
            double mu_Ry = -s_Ry / s_R * mu_R__s_R;

            double f_Fz = (l_R * m * g - h * m * g * mu_Rx) / (l_F + l_R + h * (mu_Fx * Math.Cos(dl) - mu_Fy * Math.Sin(dl) - mu_Rx));
            double f_Rz = m * g - f_Fz;

            double f_Fx = mu_Fx * f_Fz;
            double f_Fy = mu_Fy * f_Fz;
            double f_Rx = mu_Rx * f_Rz;
            double f_Ry = mu_Ry * f_Rz;

            qd[0] = xd;
            qd[1] = yd;
            qd[2] = fid;
            qd[3] = (f_Fx * Math.Cos(fi + dl) - f_Fy * Math.Sin(fi + dl) + f_Rx * Math.Cos(fi) - f_Ry * Math.Sin(fi)) / m;
            qd[4] = (f_Fx * Math.Sin(fi + dl) + f_Fy * Math.Cos(fi + dl) + f_Rx * Math.Sin(fi) + f_Ry * Math.Cos(fi)) / m;
            qd[5] = ((f_Fy * Math.Cos(dl) + f_Fx * Math.Sin(dl)) * l_F - f_Ry * l_R) / I_z;
            qd[6] = (T_F - f_Fx * r) / I_F;
            qd[7] = (T_R - f_Rx * r) / I_R;
        }

        override public void Integration(MemoryPad pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {
            Qd(q, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k1);

            MathAssistant.vector_vlinear(pad.k1, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k2);

            MathAssistant.vector_vlinear(pad.k2, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k3);

            MathAssistant.vector_vlinear(pad.k3, 1, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k4);

            MathAssistant.vector_vlinear(pad.k1, 1.0 / 6.0, q, ref pad.Qd_out);
            MathAssistant.vector_vlinear(pad.k2, 1.0 / 3.0, pad.Qd_out, ref pad.Qd_in);
            MathAssistant.vector_vlinear(pad.k3, 1.0 / 3.0, pad.Qd_in, ref pad.Qd_out);
            MathAssistant.vector_vlinear(pad.k4, 1.0 / 6.0, pad.Qd_out, ref q_next);

            while (q_next[2] > Math.PI)
                q_next[2] -= MathAssistant.PI_double;
            while (q_next[2] < -Math.PI)
                q_next[2] += MathAssistant.PI_double;
        }

        static public double GetAOA(double[] q)
        {
            double dx = q[3];
            double dy = q[4];

            double direction_movement = Math.Atan2(dy, dx);
            double direction_head = q[2];




            double AOA = direction_head - direction_movement;

            while (AOA > Math.PI)
                AOA -= MathAssistant.PI_double;
            while (AOA < -Math.PI)
                AOA += MathAssistant.PI_double;
                    


            return AOA;
        }
    }
}
